/*
 * Copyright  2017-2019 NXP
 * All rights reserved.
 *
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */
#include "tos_k.h"
#include "tos_at.h"
#include "cmsis_os.h"
#include "camera_support.h"

#include "pin_mux.h"
#include "board.h"
#include "fsl_debug_console.h"
#include "fsl_lpuart.h"
#include "fsl_pxp.h"
//#include "model.h"
#include "test_lcd_data.h"

#include "test_img.h"
#include "yolo_layer.h"

/*******************************************************************************
 * Definitions
 ******************************************************************************/
#include "display_support.h"

#define USE_PXP_SCALE 1

#define APP_IMG_HEIGHT DEMO_BUFFER_HEIGHT
#define APP_IMG_WIDTH  DEMO_BUFFER_WIDTH
static dc_fb_info_t fbInfo;
//

#define APP_FRAME_BUFFER_COUNT 4
#define APP_DISP_FRAME_BUFFER_COUNT 2
/* Pixel format RGB565, bytesPerPixel is 2. */
#define APP_BPP 2
#define DISP_BPP 4
#if (FRAME_BUFFER_ALIGN > DEMO_CAMERA_BUFFER_ALIGN)
#define DEMO_FRAME_BUFFER_ALIGN FRAME_BUFFER_ALIGN
#else
#define DEMO_FRAME_BUFFER_ALIGN DEMO_CAMERA_BUFFER_ALIGN
#endif

/*******************************************************************************
 * Prototypes
 ******************************************************************************/
static void APP_BufferSwitchOffCallback(void *param, void *switchOffBuffer);
static void APP_InitCamera(void);
static void APP_InitDisplay(void);
static void APP_CSI_RGB565(void);

/*******************************************************************************
 * Variables
 ******************************************************************************/
AT_NONCACHEABLE_SECTION_ALIGN(
    static uint32_t s_disp_frameBuffer[APP_DISP_FRAME_BUFFER_COUNT][APP_IMG_HEIGHT][APP_IMG_WIDTH],
    DEMO_FRAME_BUFFER_ALIGN);
	
AT_NONCACHEABLE_SECTION_ALIGN(
#if USE_PXP_SCALE
    static uint16_t s_frameBuffer[APP_FRAME_BUFFER_COUNT][DEMO_CAMERA_HEIGHT][DEMO_CAMERA_WIDTH],
    DEMO_FRAME_BUFFER_ALIGN);
#else
static uint16_t s_frameBuffer[APP_FRAME_BUFFER_COUNT][APP_IMG_HEIGHT][APP_IMG_WIDTH],
    DEMO_FRAME_BUFFER_ALIGN);
#endif

#define OPTIMIZE __attribute__((section(".ramfunc.$SRAM_ITC")))
/*
 * When new frame buffer sent to display, it might not be shown immediately.
 * Application could use callback to get new frame shown notification, at the
 * same time, when this flag is set, application could write to the older
 * frame buffer.
 */
static volatile bool s_newFrameShown = false;

static volatile uint8_t s_lcdActiveFbIdx;
/*******************************************************************************
 * Code
 ******************************************************************************/
extern void BOARD_InitCSIPins();

/*!
 * @brief Main function
 */
int display_init(void)
{
	 /* Define the init structure for the output LED pin*/
    gpio_pin_config_t CAM_RST_config = {kGPIO_DigitalOutput, 0, kGPIO_NoIntmode};

    BOARD_EarlyPrepareCamera();
    BOARD_InitCSIPins();
 /* Init output LED GPIO. */
    GPIO_PinInit(GPIO3, 3U, &CAM_RST_config);//��������ͷ��λ����
    GPIO_PinWrite(GPIO3, 3U, 1U);//����ͷ��λ�������ߣ�����ͷ������������
    PRINTF("CSI RGB565 example start...\r\n");

    APP_InitCamera();
	
    APP_InitDisplay();


}



static void APP_InitCamera(void)
{
    const camera_config_t cameraConfig = {
        .pixelFormat   = kVIDEO_PixelFormatRGB565,
        .bytesPerPixel = APP_BPP,
        .resolution    = FSL_VIDEO_RESOLUTION(DEMO_CAMERA_WIDTH, DEMO_CAMERA_HEIGHT),
        /* Set the camera buffer stride according to panel, so that if
         * camera resoution is smaller than display, it can still be shown
         * correct in the screen.
         */
#if USE_PXP_SCALE
        .frameBufferLinePitch_Bytes = DEMO_CAMERA_WIDTH * APP_BPP,
#else
		.frameBufferLinePitch_Bytes = APP_IMG_WIDTH * APP_BPP,
#endif
        .interface                  = kCAMERA_InterfaceGatedClock,
        .controlFlags               = DEMO_CAMERA_CONTROL_FLAGS,
        .framePerSec                = 30,
    };

    memset(s_frameBuffer, 0, sizeof(s_frameBuffer));

    BOARD_InitCameraResource();

    CAMERA_RECEIVER_Init(&cameraReceiver, &cameraConfig, NULL, NULL);

    if (kStatus_Success != CAMERA_DEVICE_Init(&cameraDevice, &cameraConfig))
    {
        PRINTF("Camera device initialization failed\r\n");
        while (1)
        {
            ;
        }
    }
     else{
		  PRINTF("Camera device initialization OK!\r\n");
			PRINTF("Camera START!\r\n");
		 }
    CAMERA_DEVICE_Start(&cameraDevice);

    /* Submit the empty frame buffers to buffer queue. */
    for (uint32_t i = 0; i < APP_FRAME_BUFFER_COUNT; i++)
    {
        CAMERA_RECEIVER_SubmitEmptyBuffer(&cameraReceiver, (uint32_t)(s_frameBuffer[i]));
    }
}

static void APP_InitDisplay(void)
{

    status_t status;

	BOARD_PrepareDisplayController();
	
    status = g_dc.ops->init(&g_dc);
    if (kStatus_Success != status)
    {
        PRINTF("Display initialization failed\r\n");
        assert(0);
    }
    s_lcdActiveFbIdx = 0;
    g_dc.ops->getLayerDefaultConfig(&g_dc, 0, &fbInfo);
    fbInfo.pixelFormat = kVIDEO_PixelFormatXRGB8888;
    fbInfo.width       = DEMO_BUFFER_WIDTH;
    fbInfo.height      = DEMO_BUFFER_HEIGHT;
    fbInfo.startX      = DEMO_BUFFER_START_Y;
    fbInfo.startY      = DEMO_BUFFER_START_X;
    fbInfo.strideBytes = DEMO_BUFFER_WIDTH * DISP_BPP;
    g_dc.ops->setLayerConfig(&g_dc, 0, &fbInfo);

    g_dc.ops->setCallback(&g_dc, 0, APP_BufferSwitchOffCallback, NULL);

	PXP_Init(PXP);
	PXP_SetProcessSurfaceBackGroundColor(PXP, 0U);
	
	PXP_SetProcessSurfacePosition(PXP, 0U, 0U, DEMO_BUFFER_WIDTH - 1U, DEMO_BUFFER_HEIGHT - 1U);
	PXP_SetAlphaSurfacePosition(PXP,0xffff,0xffff, 0U, 0U);//, DEMO_BUFFER_WIDTH - 1U, DEMO_BUFFER_HEIGHT - 1U);
	PXP_EnableCsc1(PXP, false);
}

static void APP_PXP_CONV_RGB565_To_XRGB8888(uint32_t src, uint32_t dst)
{
	pxp_ps_buffer_config_t src_cfg  = {
		.swapByte    = 0,
		.bufferAddrU = 0U,
		.bufferAddrV = 0U,
		.pixelFormat = kPXP_PsPixelFormatRGB565,
	};
	
	pxp_output_buffer_config_t dst_cfg = {
		.pixelFormat    = kPXP_OutputPixelFormatARGB8888,
		.interlacedMode = kPXP_OutputProgressive,
		.buffer1Addr    = 0U,

	};
	
	/* Clear the whole output buffer. */
    //memset((void*)dst, 0, DEMO_BUFFER_WIDTH*DEMO_BUFFER_HEIGHT*DISP_BPP);
	
	src_cfg.bufferAddr = src;
#if USE_PXP_SCALE
	src_cfg.pitchBytes  = DEMO_CAMERA_WIDTH * APP_BPP;
#else
	src_cfg.pitchBytes  = APP_IMG_WIDTH * APP_BPP;
#endif
	
	dst_cfg.width = APP_IMG_WIDTH;
	dst_cfg.height = APP_IMG_HEIGHT;
	dst_cfg.pitchBytes = APP_IMG_WIDTH*DISP_BPP;
	dst_cfg.buffer0Addr = dst;
	
    PXP_SetProcessSurfaceBufferConfig(PXP, &src_cfg);
#if USE_PXP_SCALE
	PXP_SetProcessSurfaceScaler(PXP,DEMO_CAMERA_WIDTH,DEMO_CAMERA_HEIGHT,APP_IMG_WIDTH,APP_IMG_HEIGHT);
#endif
	PXP_SetOutputBufferConfig(PXP, &dst_cfg);	
	PXP_Start(PXP);
	while (!(kPXP_CompleteFlag & PXP_GetStatusFlags(PXP)))
	{
		//tos_task_delay(1);
	}
	PXP_ClearStatusFlags(PXP, kPXP_CompleteFlag);
}

OPTIMIZE void APP_CONV_RGB565_To_XRGB8888(uint32_t src, uint32_t dst)
{
	int w = DEMO_CAMERA_WIDTH;
	int h = DEMO_CAMERA_HEIGHT;
	uint16_t *srcData = (uint16_t *)src;

	memset((void*)dst, 0, DEMO_BUFFER_WIDTH*DEMO_BUFFER_HEIGHT*DISP_BPP);

	for (int i=0;i<h;i++)
	{
		for (int j=0;j<w;j++)
		{
			uint16_t rgb565 = srcData[i * w + j];
			uint32_t *disp = dst + i*APP_IMG_WIDTH*DISP_BPP+j*DISP_BPP;
			uint8_t r = ((rgb565 >> 11) << 3) & (0xff);
			uint8_t g = ((rgb565 >> 5) << 2) & (0xff);
			uint8_t b = (rgb565 << 3) & (0xff);
			*disp = (r << 16) | (g << 8) | b;
		}
	}
}

static inline void IMAGE_DrawPixel(uint16_t *pDst, uint32_t x, uint32_t y, uint32_t col, uint16_t lcd_w)
{
    pDst[y * (lcd_w) + x] = col;
}

OPTIMIZE void IMAGE_DrawLine(uint16_t* pDst,uint32_t x0, uint32_t y0, uint32_t x1, uint32_t y1, uint16_t color, uint32_t lcd_w)
{
    uint32_t x;
    float dx, dy, y, k;
    dx = x1 - x0;
    dy = y1 - y0;
    if (dx != 0)
    {
        k = dy / dx;
        y = y0;
        for (x = x0; x < x1; x++)
        {
            IMAGE_DrawPixel(pDst, x, (uint32_t)(y + 0.5f), color, lcd_w);
            y = y + k;
        }
    }
    else
    {
        x = x0;
        for (y = y0; y < y1; y++)
        {
            IMAGE_DrawPixel(pDst, x, (uint32_t)y, color, lcd_w);
        }
    }
}

OPTIMIZE void IMAGE_DrawRect(uint16_t *pdst, uint32_t x0, uint32_t y0, uint32_t w, uint32_t h, uint16_t color, uint32_t lcd_w)
{
    IMAGE_DrawLine(pdst, x0, y0, x0 + w, y0, color, lcd_w);
    IMAGE_DrawLine(pdst, x0, y0, x0, y0 + h, color, lcd_w);
    IMAGE_DrawLine(pdst, x0, y0 + h, x0 + w, y0 + h, color, lcd_w);
    IMAGE_DrawLine(pdst, x0 + w, y0, x0 + w, y0 + h, color, lcd_w);
}

//void debug_log_printf(const char* s)
//{
//    PRINTF(s);
//}
//yolo_region_layer r1;
static inline void _itof(float* dst , uint8_t* src, uint32_t size, float div)
{
	if(div==0) return;
	else
	{
		for(int i=0 ; i<size ; i++)
		{
			dst[i]=(src[i]/2-128);
			//dst[i]=0;
			//dst[i]=src[i];
		}
	}
}
float input_float_buf[160*160];
uint8_t input_gray_buf[320*240];
uint8_t input_gray_160[160*160];
void rgb5652gray(uint16_t *src, unsigned char *dst, int width, int height)
{
    int r, g, b;
    for (int i=0; i<width*height; ++i)
    {
        r=((*(src+i))>>11)&0x1F;//  11 1111
        g=((*(src+i))>>5)&0x3F;
        b=((*(src+i))>>0)&0x1F;
        // build weighted average:
        *dst++ = (r * 76 + g * 150 + b * 30) >> 8;
    }
}

int is_in_array(short x, short y, short height, short width)
{
    if (x >= 0 && x < width && y >= 0 && y < height)
        return 1;
    else
        return 0;
}

void bilinera_interpolation(uint8_t* in_array, short height, short width,
                            uint8_t* out_array, short out_height, short out_width)
{
    double h_times = (double)out_height / (double)height,
           w_times = (double)out_width / (double)width;
    short  x1, y1, x2, y2, f11, f12, f21, f22;
    double x, y;

    for (int i = 0; i < out_height; i++){
        for (int j = 0; j < out_width; j++){
            x = j / w_times;
            y = i / h_times;

            x1 = (short)(x - 1);
            x2 = (short)(x + 1);
            y1 = (short)(y + 1);
            y2 = (short)(y - 1);
            f11 = is_in_array(x1, y1, height, width) ? in_array[y1*width+x1] : 0;
            f12 = is_in_array(x1, y2, height, width) ? in_array[y2*width+x1] : 0;
            f21 = is_in_array(x2, y1, height, width) ? in_array[y1*width+x2] : 0;
            f22 = is_in_array(x2, y2, height, width) ? in_array[y2*width+x2] : 0;
            out_array[i*out_width+j] = (uint8_t)(((f11 * (x2 - x) * (y2 - y)) +
                                       (f21 * (x - x1) * (y2 - y)) +
                                       (f12 * (x2 - x) * (y - y1)) +
                                       (f22 * (x - x1) * (y - y1))) / ((x2 - x1) * (y2 - y1)));
        }
    }
}

extern void mqttclient_task_send_count(uint32_t count);
void debug_log_printf(const char* s)
{
    PRINTF(s);
}
yolo_region_layer r1;

uint16_t picture_data[320*240]={0};
static void APP_CSI_RGB565(void)
{
    uint32_t cameraReceivedFrameAddr;
	uint32_t lcdFrameAddr;
	s_newFrameShown = false;
	uint32_t bg_color = 0x00ff0000;
	for (int i=0;i< APP_IMG_HEIGHT;i++)
	{
		for (int j=0;j<APP_IMG_WIDTH;j++)
		{
			s_disp_frameBuffer[0][i][j]= bg_color;
		}
	}
    g_dc.ops->setFrameBuffer(&g_dc, 0, (void *)s_disp_frameBuffer[0]);

    /* For the DBI interface display, application must wait for the first
     * frame buffer sent to the panel.
     */
    if ((g_dc.ops->getProperty(&g_dc) & kDC_FB_ReserveFrameBuffer) == 0)
    {
        while (s_newFrameShown == false)
        {
        	//tos_task_delay(1);
        }
    }
    s_newFrameShown = true;
    g_dc.ops->enableLayer(&g_dc, 0);
	s_newFrameShown = true;	
    CAMERA_RECEIVER_Start(&cameraReceiver);
    s_lcdActiveFbIdx = 0;
    TOS_CPU_CPSR_ALLOC();
    RegisterDebugLogCallback(debug_log_printf);
    //data ready
	for(int i=0;i<320*240;i++)
	{
		picture_data[i] = ((uint16_t*)gImage_test_lcd_data)[i];
	}
     // yolo_box
    yolo_box *boxs = NULL;
    yolo_box *p  = NULL;
    yolo_region_layer_init(&r1,125,0.12,1,5);
    int _x1 = 0;
    int _y1 = 0;
    int _x2 = 0;
    int _y2 = 0;
    setup();
    while (1)
    {
        /* Wait to get the full frame buffer to show. */
    	int status = -1;
        while (kStatus_Success != status)
        {
        	TOS_CPU_INT_DISABLE();
        	status = CAMERA_RECEIVER_GetFullBuffer(&cameraReceiver, &cameraReceivedFrameAddr);
        	TOS_CPU_INT_ENABLE();
        	tos_task_delay(1);
        }

        uint32_t start = tos_systick_get();
        boxs = (yolo_box *)loop();
        yolo_decode((float*) boxs);
        do_nms_sort(&r1,boxs);
        p = boxs;
        cameraReceivedFrameAddr = (uint32_t)picture_data;
        for(int i=0;i<125;i++){
                           p = &boxs[i] ;
                           //PRINTF("class score=%f , objectbess=%f\n" , p->class_score,p->objectness);
                           if((p->class_score * p->objectness)>0.12){
                           _x1 = (int)(p->x*320 - (p->w*320*0.5));  _x1 = _x1>0 ? _x1:1; _x1 = _x1<320 ? _x1:319;
                           _y1 = (int)(p->y*240 - (p->h*160*1.5*0.5)); _y1 = _y1>0 ? _y1:1; _y1 = _y1 < 240 ? _y1:239;
                           _x2 = (int)(p->x*320 + (p->w*320*0.5)); _x2 = _x2>0 ? _x2:1; _x2 = _x2<320 ? _x2:319;
                           _y2 = (int)(p->y*240 + (p->h*160*1.5*0.5)); _y2 = _y2>0 ? _y2:1; _y2 = _y2 < 240 ? _y2:239;
                           IMAGE_DrawRect((void*)cameraReceivedFrameAddr, _x1, _y1, _x2 - _x1, _y2 - _y1, 0x7e0, DEMO_CAMERA_WIDTH);
                           PRINTF("rectangle %d: %d,%d,%d,%d\r\n", i, _x1, _y1, _x2, _y2);
                           }
                   }
		uint32_t end = tos_systick_get()-start;
		PRINTF("model run over , cost:%d\n" , end);

        /* Wait for the previous frame buffer is shown. */
        while (s_newFrameShown == false)
        {
        	tos_task_delay(1);
        }
		lcdFrameAddr = (uint32_t)s_disp_frameBuffer[s_lcdActiveFbIdx^1];
		APP_PXP_CONV_RGB565_To_XRGB8888(cameraReceivedFrameAddr,(uint32_t)lcdFrameAddr);
		//APP_CONV_RGB565_To_XRGB8888(cameraReceivedFrameAddr,(uint32_t)lcdFrameAddr);
		TOS_CPU_INT_DISABLE();
		CAMERA_RECEIVER_SubmitEmptyBuffer(&cameraReceiver, (uint32_t)cameraReceivedFrameAddr);
		TOS_CPU_INT_ENABLE();
		s_newFrameShown = false;
		//PRINTF("Add Camera Buffer=0x%x\r\n",cameraReceivedFrameAddr);
		g_dc.ops->setFrameBuffer(&g_dc, 0, (void *)lcdFrameAddr);
		while(1){}
		tos_task_delay(20);
    }
}

static void APP_BufferSwitchOffCallback(void *param, void *switchOffBuffer)
{
		s_newFrameShown = true;
		s_lcdActiveFbIdx ^= 1;
}

void display_entry(void *parameter)
{
	display_init();
	APP_CSI_RGB565();
}
